#!env python
# -*- coding: utf-8 -*-
#	 2010.07.16
#    2014.7.30
#
#	安全制动模型的相关计算
# 
import sys
import math
import lxml.etree as etree  # 使用该扩展xml库

ENABLE_DEBUG=0
ENABLE_REPORT=0

def tocms(v):
    return v/0.036

def toms(v):
    return v/3.6

def tokmh(v):
    return v*0.036

MAX_SPEED=tocms(88)
MAX_TARGET_S=35000
LINE_SPEED=80
OP_DELTA=2.5

class core_module():
    def __init__(self, ref_acc=-0.8, real_acc=-1.0, verrkm=2.5):
        self.tract_a = [1.11,1.11,1.1,1.1,1.1,1.1,1.1,1.1,1.1,1.1,1.1,0.92,0.74,0.63,0.55,0.47,0.4,0.4,0.4,0.4,0.4,0.4,0.4,0.4]
        self.ref_acc = ref_acc
        self.real_acc = real_acc
        self.verr = toms(verrkm)
        self.t1 = 1.5
        self.t2 = 0.5
        self.t3 = 1
        self.a1 = 111
        self.a2 = 0
        self.a3 = self.real_acc/2
        self.max_speed = 120
        self.sep = 1

        self.ato_acc = 0
        self.counter = 0
        self.s_cut = 0
        self.s_safe = 0
        self.s_eb = 0
        self.s_bad = 0
        self.a1 = 0
        self.critical_v = 0
        self.ceiling_v = 0
        self.target_limit_v = 0
        self.max_ramp_a = 30
        self.trigger_time_thr = 2
        self.s0 = 0
        self.critical_delta_s = 0
        self.op_s = 0

    def get_tract_acc(self, v):
        tempi = int(v*3.6/5)
        if (tempi < 0):
            tempi = 0
        if (tempi < len(self.tract_a)):		
            return self.tract_a[tempi]
        else:
            return self.tract_a[-1]

    def set_param(self, ramp_a, t1, t2, t3, a1, a2):
        self.ramp_a = ramp_a
        self.t1 = t1
        self.t2 = t2
        self.t3 = t3
        self.a1 = a1
        self.a2 = a2

    def set_param2(self, ramp_a, t1, t2, t3, a1, a2, ato_ref):
        self.ramp_a = ramp_a
        self.t1 = t1
        self.t2 = t2
        self.t3 = t3
        self.a1 = a1
        self.a2 = a2
        self.ato_acc = ato_ref

    def get_param(self):
        return ""

    def load_xml(self, filename):
        print filename
        try:
            element_tree = etree.parse(filename)
            root = element_tree.getroot()
            for d in root:
                print d.tag, d.text
                if (isinstance(d.tag, str)):
                    if d.tag == 'max_tract':
                        self.tract_a = [float(t) for t in d.text.split(',')]
                    elif d.tag == 'ramp':
                        self.ramp_a = float(d.text)
                    elif d.tag == 'max_speed':
                        self.max_speed = int(d.text)
                    else:
                        setattr(self, d.tag, float(d.text))
        except IOError, e:
            print e

    def write_xml(self, filename):
        pass

    def get_decelerate_speed(self, target_s, target_v):
        # 计算匀减速的触发速度
        return math.sqrt(target_v*target_v-2*(self.ref_acc+self.ramp_a)*target_s)

    def get_ceiling_speed_cms(self, limit_speed):
        # 计算顶棚条件下的速度触发数值
        self.ceiling_v = limit_speed - (self.get_tract_acc(limit_speed)+self.ramp_a)*self.t1 - (self.a2+self.ramp_a)*self.t2
        return self.ceiling_v

    def get_ceiling_speed(self, limit_speed):
        return tokmh(self.get_ceiling_speed_cms(tocms(limit_speed)))

    def get_old_critical_speed_cms(self, target_s, target_v):
        """
        根据旧的列车模型计算出来的数据，需修正
        """
        v1=self.get_decelerate_speed(target_s, target_v)
        self.a1 = self.get_tract_acc(v1)
        self.critical_v = v1 + (self.ref_acc)*(self.t1 + self.t2 + self.t3) \
                - self.a1*self.t1 \
                - self.a2 * self.t2  \
                - self.a3*self.t3
        if self.critical_v < 0:
            self.critical_v = 0
            if ENABLE_DEBUG==1:
                print "v1:",v1,tokmh(v1),"tv:",self.critical_v,tokmh(self.critical_v)

        return self.critical_v

    # 计算IEEE1474触发速度
    def get_critical_speed_cms(self, target_s, target_v):
        # 计算核心的触发速度
        v5 = float(self.get_decelerate_speed(target_s, target_v))
        a1 = float((self.get_tract_acc(v5)+self.ramp_a))
        t1 = float(self.t1)
        a2 = float(self.a2 + self.ramp_a)
        t2 = float(self.t2)
        a3 = float(self.a3 + self.ramp_a)
        a4 = float(self.ref_acc + self.ramp_a)

        temp1 = float((a1-a4)*t1 + (a2-a4)*t2 + (a3-a4)*t3)
        temp2 = float(a1*t1+a2*t2+a3*t3)
        temp3 = a4*(a1*(t1*t1+2*t1*t2+2*t1*t3) + a2*(t2*t2+2*t2*t3) + a3*(t3*t3))

        temp = v5*v5 + temp3 + temp1*temp1 - temp2*temp2

        self.critical_v = math.sqrt(temp) - ((a1-a4)*t1 + (a2-a4)*t2 + (a3-a4)*t3)
        if self.critical_v < 0:
            self.critical_v = 0
            if ENABLE_DEBUG==1:
                print "v1:",v1,tokmh(v1),"tv:",self.critical_v,tokmh(self.critical_v)
                return self.critical_v

    def get_critical_speed(self, target_s, target_v):
        # 计算核心的触发速度
        target_s = target_s
        target_v = tocms(target_v)
        if ENABLE_DEBUG==1:
            print "tgt s:",target_s,"tgt v:",target_v
            return tokmh(self.get_critical_speed_cms(target_s, target_v))

    # 获取前方存在目标点时的触发速度
    def get_target_limit_speed_cms(self, target_s, target_v, limit_v):
        if target_s>MAX_TARGET_S:
            return self.get_ceiling_speed_cms(limit_v)
        else:
            tempv = self.get_critical_speed_cms(target_s, target_v)
            ceiling_v = self.get_ceiling_speed_cms(limit_v)
            if ENABLE_DEBUG==1:
                print "tgt limit", tempv, ceiling_v
                if (tempv > ceiling_v):
                    tempv = ceiling_v
                    if (self.s0 == 0 and tempv < ceiling_v):
                        self.s0 = target_s
                        self.v0 = self.get_op_speed(self.get_safe_speed(tempv))
                        self.target_limit_v = tempv
                        return self.target_limit_v

    def get_target_limit_speed(self, target_s, target_v, limit_v):
        v = self.get_target_limit_speed_cms(target_s, tocms(target_v), tocms(limit_v))
        return tokmh(v)

    def get_safe_speed(self, v):
        # 考虑平坡加速
        tempv = v - self.max_ramp_a*self.trigger_time_thr
        return tempv

    def get_op_speed(self, v):
        tempv = v - tocms(OP_DELTA)
        return tempv

    def get_all_speed(self, target_s, target_v):
        target_v = tocms(target_v)
        v1 = self.get_decelerate_speed(target_s, target_v)
        if (v1>MAX_SPEED):
            v1 = MAX_SPEED
            v2 = self.get_target_limit_speed_cms(target_s, target_v, tocms(LINE_SPEED))
            v22 = self.get_old_critical_speed_cms(target_s, target_v)
            v3 = self.get_safe_speed(v2)
            vc2_temp = (target_s-self.critical_delta_s)*-2*(self.ato_acc+self.ramp_a)
            if (vc2_temp>0):
                vc2 = math.sqrt(vc2_temp)
            else:
                vc2 = 0
                v4 = self.get_op_speed(v3)
                # 撞线后减速
                if (self.s0 == 0):
                    vato = v4
                else:
                    temp = self.v0*self.v0 + 2.0*(self.ato_acc+self.ramp_a)*(self.s0 - target_s)
                    if temp>0:
                        vato = math.sqrt(temp)
                    else:
                        vato = 0
                        if target_s > self.critical_delta_s:
                            vato3 = math.sqrt(-2.0*(self.ref_acc+self.ramp_a)*(target_s - self.critical_delta_s))
                        else:
                            vato3 = 0
                            # 
                            if target_s > self.op_s:
                                vato2 = math.sqrt(-2.0*(self.ato_acc+self.ramp_a)*(target_s-self.op_s))
                            else:
                                vato2 = 0
                                print '%3.1f	%3.1f	%3.1f	%3.1f	%3.1f	%3.1f	%3.1f	%3.1f	%3.1f	%3.1f	%3.1f	%3.1f'%\
                                        (target_s/100.0, tokmh(v1), tokmh(v2), tokmh(v22), tokmh(vc2), tokmh(vato), tokmh(v3), tokmh(v4), tokmh(vato3), \
                                         v1, v2, v3)

    def get_critical_s(self):
        # 获取ieee1474触发曲线和终点的距离
        a1 = self.get_tract_acc(0)
        self.a1 = a1
        v2 = (self.ramp_a+a1)*self.t1
        v3 = v2 + (self.a2 + self.ramp_a)*self.t2
        v4 = v3 + (self.a3 + self.ramp_a)*self.t3
        #print v2, v3, v4
        s = (a1+self.ramp_a)*self.t1*self.t1/2.0 \
                + v2*self.t2 + (self.a2 + self.ramp_a)*self.t2*self.t2/2.0 \
                + v3*self.t3 + (self.a3 + self.ramp_a)*self.t3*self.t3/2.0 \
                - v4*v4/(self.ref_acc+self.ramp_a)/2
        self.critical_delta_s = s
        return s

    def get_refreal_delta_s(self, v, safety_margin=15, ramp_a=0):
        """
        计算某速度下，参考距离和实际距离的误差，也就是overlap的原始数据
        """
        return self.get_realeb_s(v, safety_margin, ramp_a) - self.get_refeb_s(v, ramp_a)

    def get_refeb_s(self, v, ramp_a=0):
        """
        计算按照参考减速度停下来的距离
        """
        s = -v*v/(self.ref_acc+ramp_a)/2
        return s

    def get_realeb_s(self, v, safety_margin=15, ramp_a=0):
        """
        计算从触发紧急制动到停下来的距离
        """
        v1 = v + toms(self.verr)
        a1 = self.get_tract_acc(v1)
        v2 = v1 + (ramp_a+a1)*self.t1
        v3 = v2 + (self.a2 + ramp_a)*self.t2
        v4 = v3 + (self.a3 + ramp_a)*self.t3
        #print v2, v3, v4
        s = v1*self.t1 + (a1+ramp_a)*self.t1*self.t1/2.0 \
                + v2*self.t2 + (self.a2 + ramp_a)*self.t2*self.t2/2.0 \
                + v3*self.t3 + (self.a3 + ramp_a)*self.t3*self.t3/2.0 \
                - v4*v4/(self.real_acc+ramp_a)/2
        return s + safety_margin

    def print_param(self):
        print self.t1, self.t2, self.t3, self.a1, self.ref_acc, self.real_acc

    def get_overlap(self, vkm, safety_margin=15.0, ramp_a=0, step=1):
        #self.print_param()
        result = ""
        detail = ""
        max_delta = 0
        max_v = 0
        for v0 in range(vkm, 5-step, -step):
            v = toms(v0)
            s1 = self.get_realeb_s(v, safety_margin, ramp_a)
            s2 = self.get_refeb_s(v, ramp_a)
            delta = s1-s2
            rstr = u"入口v:%d, 参考s:%.2f, 实际s: %.2f, 距离误差: %.2f\r\n"%(v0, s2, s1, delta)
            detail += rstr
            if (delta > max_delta):
                max_delta = delta
                max_v = v0

        # 统计结果
        result = u"保护区段:%3.2f @:%d km/h"%(max_delta, max_v)
        print result
        print detail
        return result, detail
        #return result.decode('utf-8'), detail.decode('utf-8')

    def get_op_s(self):
        v1 = toms(OP_DELTA) + self.max_ramp_a*self.trigger_time_thr
        a1 = self.get_tract_acc(0)
        v2 = v1 + (self.ramp_a+a1)*self.t1
        v3 = v2 + (self.a2 + self.ramp_a)*self.t2
        v4 = v3 + (self.a3 + self.ramp_a)*self.t3
        #print v2, v3, v4
        s = v1*self.t1 + (a1+self.ramp_a)*self.t1*self.t1/2.0 \
                + v2*self.t2 + (self.a2 + self.ramp_a)*self.t2*self.t2/2.0 \
                + v3*self.t3 + (self.a3 + self.ramp_a)*self.t3*self.t3/2.0 \
                - v4*v4/(self.ref_acc+self.ramp_a)/2
        self.op_s = s
        return s

    def get_loop_all_speed(self, s1, target_v):
        if (s1>100):
            if ENABLE_REPORT==1:
                self.report()
            else:
                print '距离	匀减速触发'+'%3.1f'%(self.ref_acc)+'	1474EB触发	1474EB_old	危险点反推'+'%3.1f'%(self.ato_acc)+'	触发点正推	全常用制动	推荐速度	推荐反推'
                for i in range(int(s1), -1, -100):
                    self.get_all_speed(i, target_v)

    def get_trigger_speed(self, s, hasoverlap):
        # 获取安全相关速度
        return 0

    def report(self):
        print '# t1:', self.t1, ' t2:', self.t2, ' t3:', self.t3, 'a2:', self.a1, self.a2, 'a3:', self.ref_acc/2, 'ramp_a:', self.ramp_a
        print '# REF_A:',self.ref_acc, ' REAL_A:', self.real_acc, 'TRIGGER_V:', self.v_safe*0.036, 'delta_s', self.get_critical_s(), self.get_op_s()

if __name__ == "__main__":
    # 输入
    ref_a = -0.85
    ato_acc = -0.8
    real_a = -1.0
    v0 = 0.0
    v = 75.0
    verr = 2.5
    t1 = 1.5	# 切除牵引时间
    t2 = 0.2	# 惰行时间
    t3 = 0.9	# eb建立时间
    a1 = 1.1
    a2 = 0
    ramp_a =0


    module = core_module(ref_a, real_a)
    module.set_param2(ramp_a, t1, t2, t3, a1, a2, ato_acc)
    module.get_overlap(100, 15, ramp_a)
